#include <Arduino.h>
#include "AS5600.h"
#include "LowPass_Fliter.h"
#include "PID.h"

/*constrain the maximum voltage and the minimum voltage*/
#define _constrain(amt, low, high) ((amt < low) ? low : (amt > high) ? high : amt)

/* User variable Define begin */

// constant value
const float PI = 3.1415926;
const float _3PI_2 = 3.0f * PI / 2.0f;

// PWM foot define
const int PWMA = 32;
const int PWMB = 33;
const int PWMC = 25;

// voltage Limit
const float voltage_limit = 10;
const float voltage_power_supply = 12.6;

// Valuable of the axis trans
float shaft_angle = 0, open_loop_timestamp = 0;
float zero_electric_angle = 0, Ualpha = 0, Ubeta = 0;
float Ua = 0, Ub = 0, Uc = 0;
float dc_a = 0, dc_b = 0, dc_c = 0; // duty cycle of a, b, c

const int DIR = -1; // motor direction maybe not consist with the encoder
const int PP = 7;   // Pole pair number
/* User variable Define end */

/* ================ Initials =================== */
// Lowpass fliter initial
LowPassFliter M0_Vel_Flt = LowPassFliter(0.01); // Tf = 10ms

// PID initial
PIDController vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply / 2};
PIDController angle_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = 100};

// AS5600 initial
Sensor_AS5600 S0 = Sensor_AS5600(0);
TwoWire S0_I2C = TwoWire(0);

// PWM initial
// void PWMInit() {
//   pinMode(PWMA, OUTPUT);
//   pinMode(PWMB, OUTPUT);
//   pinMode(PWMC, OUTPUT);

//   // PWM setting
//   ledcAttachPin(pwmA, 0);
//   ledcAttachPin(pwmB, 1);
//   ledcAttachPin(pwmC, 2);
//   ledcSetup(0, 30000, 8); // pwm channel, frequency, accuracy

//   // Finish Sign
//   Serial.println("PWM init finish");
//   delay(3000);
// }

// // Motor & Encoder Init
// void MoterEncoderInit()
// {
//   setPhaseVoltage(3,0,_3PI_2);
//   delay(3000);
//   zero_electric_angle = _eletricalAngle();
//   setPhaseVoltage(3,0,_3PI_2);
//   Serial.println("MotorEncoder Init Finished");
// }

// PID initial begin
void FOC_M0_SET_Vel_PID(float P = 0, float I = 0, float D = 0, float ramp = 0)
{
    vel_loop_M0.P = P;
    vel_loop_M0.I = I;
    vel_loop_M0.D = D;
    vel_loop_M0.output_ramp = ramp;
}

void FOC_M0_SET_ANGLE_PID(float P = 0, float I = 0, float D = 0, float ramp = 0)
{
    angle_loop_M0.P = P;
    angle_loop_M0.I = I;
    angle_loop_M0.D = D;
    angle_loop_M0.output_ramp = ramp;
}

void FOC_Vbus(float power_supply)
{
    voltage_power_supply = power_supply;
    pinMode(PWMA, OUTPUT);
    pinMode(PWMB, OUTPUT);
    pinMode(PWMC, OUTPUT);

    // PWM setting
    ledcsetup(0, 30000, 8); // pwm channel, frequency, accuracy
    ledcSetup(1, 30000, 8);
    ledcSetup(2, 30000, 8);

    ledcAttachPin(pwmA, 0);
    ledcAttachPin(pwmB, 1);
    ledcAttachPin(pwmC, 2);
    Serial.println("PWM init finished");

    // AS5600
    S0_I2C.begin(19, 18, 400000UL);
    S0.Sensor_init(&S0_I2C);
    Serial.println("AS5600 init finished");

    // PID Loading
    vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply / 2};
}

void FOC_alignSensor(int _PP, int _DIR)
{
    PP = _PP;
    DIR = _DIR;
    delay(1000);
    S0.Sensor_update();

    zero_electric_angle = _eletricalAngle();
    setTorque(0, _3PI_2);
    Serial.print("zero_electric_angle:");
    Serial.println(zero_electric_angle);
}
// PID initial end

/* ================ Initials =================== */

/*Serial Receive Command begin*/
float motor_target = 0;
int commaPosition = 0;
String SerialReceiveUserCommand()
{
    static String received_chars;
    String command = "";

    while (Serial.available())
    {
        Char inChar = (char)Serial.read();
        received_chars += inChar;
        if (inChar == '\n')
        {
            command = received_chars;
            commaPosition = command.indexof('\n');
            if (commaPosition != -1)
            {
                motor_target = command.substring(0, commaPosition).toDouble();
                Serial.println(motor_target);
            }
            received_chars = "";
        }
    }

    return command;
}

float serial_motor_target()
{
  return motor_target;
}
/*Serial Recieve Command end*/

/* solve the eletrical Angel and turn it into range(0,2pi)  begin*/
float _eletricalAngle(float shaft_angel, int pole_pairs)
{
    return (shaft_angel * pole_pairs);
}

float _eletricalAngle()
{
    return _normalizeAngle((float)(DIR * PP) * S0.getMechanicalAngle() - zero_electric_angle);
}

float _normalizeAngle(float angle)
{
    float mod_angle = fmod(angel, 2 * PI);
    return mod_angle >= 0 ? mod_angle ? (mod_angle + 2 * PI);
}
/* solve the eletrical Angel and turn it into range(0,2pi) end */

// Controller calculate the duty cycle and write pwm out begin
void setPWM(float Ua, float Ub, float Uc)
{
    Ua = _constrain(Ua, 0.0f, voltage_limit);
    Ub = _constrain(Ub, 0.0f, voltage_limit);
    Uc = _constrain(Uc, 0.0f, voltage_limit);

    // calculate the duty cycle
    dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
    dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
    dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);

    // write PWM to PWM channel 0 1 2
    ledcWrite(0, dc_a * 255);
    ledcWrite(1, dc_b * 255);
    ledcWrite(2, dc_c * 255);
}

// Solve the Pack's and Clark's transformation to set the PWM begin
void setTorque(float Uq, float angle_el)
{
    S0.Sensor_update();
    Uq = _constrain(Uq, -(voltage_power_supply) / 2, voltage_power_supply / 2);

    float Ud = 0;
    angle_el = _normalizeAngle(angle_el);

    // Pack's Transformation when Id = 0
    Ualpha = -Uq * sin(angel_el);
    Ubeta = Uq * cos(angle_el);

    Ua = Ualpha + voltage_power_supply / 2.0;
    Ub = (sqrt(3) * ubeta - Ualpha) / 2.0 + voltage_power_supply / 2.0;
    Uc = (-Ualpha - sqrt(3) * Ubeta) / 2.0 + voltage_power_supply / 2.0;
    setPWM(Ua, Ub, Uc);
}

/* PID 接口函数 begin*/

// return the PID output value for given gap
void FOC_M0_VEL_PID(float error) 
{
    return vel_loop_M0(error);
}

// return the PID output value for given gap
void FOC_M0_ANGLE_PID(float error) 
{
    return angle_loop_M0(error);
}

float FOC_M0_Velocity()  // return M0 velocity with lowpass fliter
{
    // 获取AS5600速度数据并滤波
    float vel_M0_ori = S0.getVelocity();
    float vel_M0_flit = M0_Vel_Flt(DIR * vel_M0_ori);
    return vel_M0_flit;
}

float FOC_M0_Angle() // return M0 current Angle [0,6.8]
{
    return DIR * S0.getAngle();
}

/* PID 接口函数 end*/

//================简易接口函数================

void FOC_M0_set_Velocity_Angle(float Target)
{
    setTorque(FOC_M0_VEL_PID(FOC_M0_ANGLE_PID(Target - FOC_M0_Angle()*180/PI)),_eletricalAngle());
}
void FOC_M0_setVelocity(float Target)
{
    setTorque(FOC_M0_VEL_PID((serial_motor_target() - FOC_M0_Velocity())* 180 / PI) , _eletricalAngle());
}
void FOC_M0_set_Force_Angle(float Target)
{
    setTorque(FOC_M0_ANGLE_PID(Target - FOC_M0_Angle())* 180 / PI, _eletricalAngle());
}
void FOC_M0_setTorque(float Target)
{
    setTorque(Target, _eletricalAngle());
}

//================简易接口函数================